#include "motor_drv.hpp"

Motor_drv::Motor_drv(std::string port, int baud)
    : Serial(port, baud)
{
    open();
    runThreadFunc();
    startTimer(2);
}
void Motor_drv::set_speed(int sp,int angle)
{
    int tiao,temp;
    temp=(angle-angle_cur);
    tiao=temp/24;
    if(tiao>600)tiao=600;
    if(tiao<-600)tiao=-600;
    if(sp==0)
    {
        if((temp<500)&&(temp>-500))
         tiao=0;
    }
    speed_set0=-sp+tiao;
    speed_set1=sp+tiao;
   // if(speed_set0==0)
    std::cout<<"lun0="<<speed_set0<<"lun1="<<speed_set1<<std::endl;
   // if(speed_set1==0)std::cout<<"lun1="<<std::endl;
}
void Motor_drv::startTimer(int ms_s)
{
    interval = std::chrono::milliseconds(ms_s);
     if (!running) {
            running = true;
            auto thread = std::thread([this]() {
                while (running) {
                 sendbuf_vect.clear();   
                 set_send_v(Fucn_flag,0);
                 write(sendbuf_vect);
                 Fucn_flag++;
                 if(Fucn_flag>2)Fucn_flag=0;
                 std::this_thread::sleep_for(interval);
                }
     });
      thread.detach(); // 让定时器在后台运行，防止主线程阻塞
    }
}
void Motor_drv::set_send_v(int n,int id)
{
    u_char ch;
    int16_t temp;
    switch(n)
    {
        case 0://读编码器角度
            sendbuf_vect.push_back(0xaa);
            ch=bianma_id>>8;
            sendbuf_vect.push_back(ch);
            ch=bianma_id&0xff;
            sendbuf_vect.push_back(ch);
            sendbuf_vect.push_back(0x01);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            ch=0;
            for(int i=0;i<11;i++)ch+=sendbuf_vect[i];
            sendbuf_vect.push_back(ch);
        break;
        case 1:  //轮子0速度
            sendbuf_vect.push_back(0xaa);
            ch=motor_lun0_id>>8;
            sendbuf_vect.push_back(ch);
            ch=motor_lun0_id&0xff;
            sendbuf_vect.push_back(ch);
            
            //速度
            sendbuf_vect.push_back(0x00);
            temp=speed_set0;
            ch=temp>>8;
            sendbuf_vect.push_back(ch);
            ch=temp&0xff;
            sendbuf_vect.push_back(ch);
            //电流
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            //
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            
            sendbuf_vect.push_back(0x00);
        // sendbuf_vect.push_back(0x00);
            ch=0;
            for(int i=0;i<11;i++)ch+=sendbuf_vect[i];
            //ch=0x0bd+(id&0xff);
            sendbuf_vect.push_back(ch);
        break;
        case 2:  //轮子1速度
            sendbuf_vect.push_back(0xaa);
            ch=motor_lun1_id>>8;
            sendbuf_vect.push_back(ch);
            ch=motor_lun1_id&0xff;
            sendbuf_vect.push_back(ch);
            
            //速度
            sendbuf_vect.push_back(0x00);
            temp=speed_set1;
            ch=temp>>8;
            sendbuf_vect.push_back(ch);
            ch=temp&0xff;
            sendbuf_vect.push_back(ch);
            //电流
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            //
            sendbuf_vect.push_back(0x00);
            sendbuf_vect.push_back(0x00);
            
            sendbuf_vect.push_back(0x00);
        // sendbuf_vect.push_back(0x00);
            ch=0;
            for(int i=0;i<11;i++)ch+=sendbuf_vect[i];
            //ch=0x0bd+(id&0xff);
            sendbuf_vect.push_back(ch);
        break;
    }
}
int Motor_drv::rec_ok(std::vector<u_int8_t> &msg)
{
      int flag=0;
    u_int8_t ch=0;
    if(msg[0]!=0xaa)return 1;
    if(msg.size()!=12)return 2;
    for(int i=0;i<11;i++)
        ch+=msg[i];
    if(ch==msg[11])    
        return flag;
      else return 3;  
}
void Motor_drv::recvMsg(std::vector<u_int8_t> &msg)
{
    //std::cout<<"hello"<<std::endl;
    int id;
    int16_t ag;
    int temp;
    
   if(rec_ok(msg)==0)
   {
        id=msg[1]*256+msg[2];
        //std::cout<<"id= "<<id<<std::endl;
        if((bianma_id+0x100)==id)
        {
            ag=msg[3]*256+msg[4];
            angle_cur=cal_angle_run(ag);
        }
        if((motor_lun0_id+0x100)==id)
        {
            ag=msg[5]*256+msg[6];
            speed_cur0=ag;
        }
        if((motor_lun1_id+0x100)==id)
        {
            ag=msg[5]*256+msg[6];
            speed_cur1=ag;
        }
   }
}

int Motor_drv::cal_angle_run(int agg)
{//左手定律
  int zero_ag=-15374-1060;
  int angle,temp;
  if(zero_ag>=0)
  {
    if(agg>=0)
    {
        angle=agg-zero_ag;
    }
    else 
    {
        temp=zero_ag-32768;
        if(temp<=agg){ angle=agg-zero_ag;}
        else
        {
            angle= 32768+agg-temp;
        }
    }
  }
  else
  {
    if(agg>=0)  //zero_ag<0 agg>0
    {
      temp=32768+zero_ag; 
      if(temp>=agg)
       {angle=agg-zero_ag;}
      else
      {
        angle=agg-32768-temp;
      } 
     
    }
    else  //zero_ag<0 agg<0
    {
      angle=agg-zero_ag;  
    }
  }
  return angle;
}